#include <ros/ros.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include "camera_select.h"
#include <geometry_msgs/PoseArray.h>
#include <tf/transform_listener.h>

cv::Mat image_left;
cv::Mat image_depth;
cv::Mat image_color;
cv::Mat image_point3d;
ros::Time stamp_cam;
std::basic_string<char> frame_id_cam;

void img_irl(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, "8UC1");
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    image_left = cv_ptr->image;
}

void img_depth(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, "16UC1");
    }
    catch (cv_bridge::Exception& e1)
    {
      ROS_ERROR("cv_bridge exception: %s", e1.what());
      return;
    }
    image_depth = cv_ptr->image;
    stamp_cam=msg->header.stamp;
    frame_id_cam=msg->header.frame_id;
}

void img_color(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, "rgb8");
    }
    catch (cv_bridge::Exception& e1)
    {
      ROS_ERROR("cv_bridge exception: %s", e1.what());
      return;
    }
    image_color = cv_ptr->image;
}

void img_point3d(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, "16SC1");
    }
    catch (cv_bridge::Exception& e1)
    {
      ROS_ERROR("cv_bridge exception: %s", e1.what());
      return;
    }
    image_point3d = cv_ptr->image;
    //std::cout<<"point3d240*320:::x::"<<image_point3d.at<short>(240,320)<<std::endl;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "bucketcheck_camera");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub_depth = it.subscribe("/camera/depth/image_raw", 1, img_depth);
  image_transport::Subscriber sub_point3d = it.subscribe("/camera/depth/p3d_y", 1, img_point3d);
//image_transport::Subscriber sub_irl = it.subscribe("/rgb/image_raw", 1, img_irl);
//image_transport::Subscriber sub_color = it.subscribe("/camera/color/image_raw", 1, img_color);

  ros::Publisher pub=nh.advertise<geometry_msgs::PoseArray>("/camera_bucket",20);

  CameraSelect bucketcheck;
  tf::TransformListener listener;
  tf::StampedTransform transform;

  geometry_msgs::PoseArray detectionInfo;
  geometry_msgs::Pose bucket2camera;
  geometry_msgs::Pose circle_center_point;
  while(ros::ok()){
    //深度图
    if(!image_depth.empty()&&!image_point3d.empty()){
        detectionInfo.poses.clear();
        bucketcheck.camera_select(image_point3d,image_depth,bucket2camera,circle_center_point);
//ROS_INFO("xxx::%f,yy:%f",bucket2camera.position.x,bucket2camera.position.y);
        detectionInfo.poses.push_back(circle_center_point);
        detectionInfo.poses.push_back(bucket2camera);
        pub.publish(detectionInfo);
//        pub.publish(bucket2camera);
//        pub_circle.publish(circle_center_point);


//        try{
//          listener.lookupTransform("/base_link","/camera",ros::Time(0),transform);
//          tf::Transform camera2baselink;

//          camera2baselink.setOrigin(transform.getOrigin());
//          camera2baselink.setRotation(transform.getRotation());

//          tf::Vector3 near_point;
//          near_point.setX(bucket2camera.position.x/100);
//          near_point.setY(bucket2camera.position.y/100);
//          near_point.setZ(0);
//          tf::Vector3 near_point_new = camera2baselink*near_point;
//          bucket2camera.position.x = near_point_new[0]*100;
//          bucket2camera.position.y = near_point_new[1]*100;
//          bucket2camera.position.z = 0;

//          tf::Vector3 center_point;
//          center_point.setX(circle_center_point.position.x/100);
//          center_point.setY(circle_center_point.position.y/100);
//          center_point.setZ(0);
//          tf::Vector3 center_point_new = camera2baselink*center_point;
//          circle_center_point.position.x = center_point_new[0]*100;
//          circle_center_point.position.y = center_point_new[1]*100;
//          circle_center_point.position.z = 0;

//          pub.publish(bucket2camera);
//          pub_circle.publish(circle_center_point);
//        }
//        catch(tf::TransformException &ex){
//          ROS_ERROR("%s",ex.what());
//          ros::Duration(0.02).sleep();
//        }


 //       std::cout<<"circle_center::"<<circle_center_point.position.x<<",,"<<circle_center_point.position.y<<std::endl;
    }
    cv::waitKey(1);
    ros::spinOnce();
  }

}
